using System;

namespace BOL.Maths.LinearAlgebra
{
	/// <summary>Singular Value Decomposition.
	/// For an m-by-n matrix A with m >= n, the singular value decomposition is
	/// an m-by-n orthogonal matrix U, an n-by-n diagonal matrix S, and
	/// an n-by-n orthogonal matrix V so that A = U*S*V'.
	/// The singular values, sigma[k] = S[k, k], are ordered so that
	/// sigma[0] >= sigma[1] >= ... >= sigma[n-1].
	/// The singular value decompostion always exists, so the constructor will
	/// never fail.  The matrix condition number and the effective numerical
	/// rank can be computed from this decomposition.
	/// </summary>
	public sealed class SingularValueDecomposition : DecompositionBase
	{
		#region Private variables
		
		private double[][] _u; // 
        private double[][] _v; // matrices U and V.
        private double[] _singularVector; // singular values.

		#endregion

		#region Public Properties

        /// <summary>Returns two norm.</summary>
        public double Norm2 { get { return _singularVector[0]; } }

        /// <summary>Returns two norm condition number.</summary>
        public double Condition { get { return _singularVector[0] / _singularVector[Math.Min(_m, _n) - 1]; } }

        /// <summary>Returns effective numerical matrix rank.</summary>
        public int Rank
        {
            get
            {
                double eps = Math.Pow(2.0, -52.0);
                double tol = Math.Max(_m, _n) * _singularVector[0] * eps;
                int r = 0;
                for (int i = 0; i < _singularVector.Length; i++)
                    if (_singularVector[i] > tol)
                        r++;
                return r;
            }
        }

        /// <summary>Returns the diagonal vector of singular values.</summary>
        public double[] SingularVector { get { return _singularVector; } }

        /// <summary>Returns the diagonal matrix of singular values</summary>
        public double[,] SingularMatrix
        {
            get
            {
                double[,] singularMatrix = new double[_n, _n];

                for (int i = 0; i < _n; i++)
                    singularMatrix[i, i] = _singularVector[i];

                return singularMatrix;
            }
        }

        /// <summary>Returns the left singular vectors, u.</summary>
        public double[][] U { get { return _u; } }

        /// <summary>Returns the right singular vectors, v.</summary>
        public double[][] V { get { return _v; } }

		#endregion

		#region Constructor

		/// <summary>Constructs the singular value decomposition</summary>
		public SingularValueDecomposition(double[][] matrix) : base(matrix)
		{
            double[][] a;
            

            if (_m < _n)
            {
                _m = matrix[0].Length;
                _n = matrix.Length;
                a = MatrixDouble.Transpose(matrix);
            }
            else
            {
                a = new double[_m][];
                for (int i = 0; i < _m; i++)
                {
                    a[i] = new double[_n];
                    Array.Copy(matrix[i], a[i], _n);
                }
            }

			int nu = Math.Min(_m, _n);
            _singularVector = new double[Math.Min(_m + 1, _n)];
			_u = new double[_m][];
            for (int i = 0; i < _m; i++)
                _u[i] = new double[nu];
            _v = new double[_n][];
            for (int j = 0; j < _n; j++)
                _v[j] = new double[_n];

            double[] e = new double[_n];
            double[] work = new double[_m];
			
            bool wantu = true;
			bool wantv = true;
			
			// Reduce A to bidiagonal form, storing the diagonal elements in s and the super-diagonal elements in e.
            int nct = Math.Min(_m - 1, _n);
            int nrt = Math.Max(0, Math.Min(_n - 2, _m));

			for (int k = 0; k < Math.Max(nct, nrt); k++)
			{
				if (k < nct)
				{
					// Compute the transformation for the k-th column and
					// place the k-th diagonal in singularVector[k].
					// Compute 2-norm of k-th column without under/overflow.
                    _singularVector[k] = 0;
                    for (int i = k; i < _m; i++)
                        _singularVector[k] = Functions.GeometryFunctions.Hypot(_singularVector[k], a[i][k]);

                    if (_singularVector[k] != 0.0)
					{
						if (a[k][k] < 0.0)
                            _singularVector[k] = -_singularVector[k];

                        for (int i = k; i < _m; i++)
                            a[i][k] /= _singularVector[k];
						a[k][k] += 1.0;
					}
                    _singularVector[k] = -_singularVector[k];
				}
                for (int j = k + 1; j < _n; j++)
				{
                    if ((k < nct) & (_singularVector[k] != 0.0))
					{
						// Apply the transformation.
						double t = 0;
                        for (int i = k; i < _m; i++)
							t += a[i][k] * a[i][j];
						t = (- t) / a[k][k];

                        for (int i = k; i < _m; i++)
							a[i][j] += t * a[i][k];
					}
					
					// Place the k-th row of A into e for the
					// subsequent calculation of the row transformation.
					e[j] = a[k][j];
				}
				if (wantu & (k < nct))
				{
					// Place the transformation in U for subsequent back
					// multiplication.
                    for (int i = k; i < _m; i++)
                        _u[i][k] = a[i][k];
				}
				if (k < nrt)
				{
					// Compute the k-th row transformation and place the
					// k-th super-diagonal in e[k].
					// Compute 2-norm without under/overflow.
					e[k] = 0;
                    for (int i = k + 1; i < _n; i++)
						e[k] = Functions.GeometryFunctions.Hypot(e[k], e[i]);

                    if (e[k] != 0.0)
					{
						if (e[k + 1] < 0.0)
							e[k] = - e[k];

                        for (int i = k + 1; i < _n; i++)
							e[i] /= e[k];

						e[k + 1] += 1.0;
					}
					e[k] = - e[k];
                    if ((k + 1 < _m) & (e[k] != 0.0))
					{
						// Apply the transformation.
                        for (int i = k + 1; i < _m; i++)
							work[i] = 0.0;

                        for (int j = k + 1; j < _n; j++)
                            for (int i = k + 1; i < _m; i++)
								work[i] += e[j] * a[i][j];

                        for (int j = k + 1; j < _n; j++)
						{
							double t = (- e[j]) / e[k + 1];
                            for (int i = k + 1; i < _m; i++)
								a[i][j] += t * work[i];
						}
					}

					if (wantv)
					{
						// Place the transformation in V for subsequent
						// back multiplication.
                        for (int i = k + 1; i < _n; i++)
                            _v[i][k] = e[i];
					}
				}
			}
			
			// Set up the final bidiagonal matrix or order p.
            int p = Math.Min(_n, _m + 1);
            if (nct < _n)
                _singularVector[nct] = a[nct][nct];

            if (_m < p)
                _singularVector[p - 1] = 0.0;

			if (nrt + 1 < p)
				e[nrt] = a[nrt][p - 1];

			e[p - 1] = 0.0;
			
			// If required, generate U.
			if (wantu)
			{
				for (int j = nct; j < nu; j++)
				{
                    for (int i = 0; i < _m; i++)
                        _u[i][j] = 0.0;

                    _u[j][j] = 1.0;
				}
				for (int k = nct - 1; k >= 0; k--)
				{
                    if (_singularVector[k] != 0.0)
					{
						for (int j = k + 1; j < nu; j++)
						{
							double t = 0;
                            for (int i = k; i < _m; i++)
                                t += _u[i][k] * _u[i][j];
                            t = (-t) / _u[k][k];

                            for (int i = k; i < _m; i++)
                                _u[i][j] += t * _u[i][k];
						}

                        for (int i = k; i < _m; i++)
                            _u[i][k] = -_u[i][k];
                        _u[k][k] = 1.0 + _u[k][k];

						for (int i = 0; i < k - 1; i++)
                            _u[i][k] = 0.0;
					}
					else
					{
                        for (int i = 0; i < _m; i++)
                            _u[i][k] = 0.0;
                        _u[k][k] = 1.0;
					}
				}
			}
			
			// If required, generate V.
			if (wantv)
			{
                for (int k = _n - 1; k >= 0; k--)
				{
					if ((k < nrt) & (e[k] != 0.0))
					{
						for (int j = k + 1; j < nu; j++)
						{
							double t = 0;
                            for (int i = k + 1; i < _n; i++)
                                t += _v[i][k] * _v[i][j];
                            t = (-t) / _v[k + 1][k];

                            for (int i = k + 1; i < _n; i++)
                                _v[i][j] += t * _v[i][k];
						}
					}

                    for (int i = 0; i < _n; i++)
						_v[i][k] = 0.0;
					_v[k][k] = 1.0;
				}
			}
			
			// Main iteration loop for the singular values.
			int pp = p - 1;
			int iter = 0;
			double eps = (double)Math.Pow(2.0, - 52.0);
			while (p > 0)
			{
				int k, kase;
				
				// Here is where a test for too many iterations would go.
				
				// This section of the program inspects for
				// negligible elements in the s and e arrays.  On
				// completion the variables kase and k are set as follows.
				
				// kase = 1     if s(p) and e[k-1] are negligible and k<p
				// kase = 2     if s(k) is negligible and k<p
				// kase = 3     if e[k-1] is negligible, k<p, and
				//              s(k), ..., s(p) are not negligible (qr step).
				// kase = 4     if e(p-1) is negligible (convergence).
				
				for (k = p - 2; k >= - 1; k--)
				{
					if (k == - 1)
					{
						break;
					}
                    if (Math.Abs(e[k]) <= eps * (Math.Abs(_singularVector[k]) + Math.Abs(_singularVector[k + 1])))
					{
						e[k] = 0.0;
						break;
					}
				}
				if (k == p - 2)
				{
					kase = 4;
				}
				else
				{
					int ks;
					for (ks = p - 1; ks >= k; ks--)
					{
						if (ks == k)
						{
							break;
						}
						double t = (ks != p?Math.Abs(e[ks]):0.0) + (ks != k + 1?Math.Abs(e[ks - 1]):0.0);
                        if (Math.Abs(_singularVector[ks]) <= eps * t)
						{
                            _singularVector[ks] = 0.0;
							break;
						}
					}
					if (ks == k)
					{
						kase = 3;
					}
					else if (ks == p - 1)
					{
						kase = 1;
					}
					else
					{
						kase = 2;
						k = ks;
					}
				}
				k++;
				
				// Perform the task indicated by kase.
				switch (kase)
				{
					// Deflate negligible s(p).
					case 1:  
					{
						double f = e[p - 2];
						e[p - 2] = 0.0;
						for (int j = p - 2; j >= k; j--)
						{
                            double t = Functions.GeometryFunctions.Hypot(_singularVector[j], f);
                            double cs = _singularVector[j] / t;
							double sn = f / t;
                            _singularVector[j] = t;
							if (j != k)
							{
								f = (- sn) * e[j - 1];
								e[j - 1] = cs * e[j - 1];
							}
							if (wantv)
							{
                                for (int i = 0; i < _n; i++)
								{
                                    t = cs * _v[i][j] + sn * _v[i][p - 1];
                                    _v[i][p - 1] = (-sn) * _v[i][j] + cs * _v[i][p - 1];
									_v[i][j] = t;
								}
							}
						}
					}
					break;
						
					// Split at negligible s(k).
					case 2:  
					{
						double f = e[k - 1];
						e[k - 1] = 0.0;
						for (int j = k; j < p; j++)
						{
                            double t = Functions.GeometryFunctions.Hypot(_singularVector[j], f);
                            double cs = _singularVector[j] / t;
							double sn = f / t;
                            _singularVector[j] = t;
							f = (- sn) * e[j];
							e[j] = cs * e[j];
							if (wantu)
							{
                                for (int i = 0; i < _m; i++)
								{
                                    t = cs * _u[i][j] + sn * _u[i][k - 1];
                                    _u[i][k - 1] = (-sn) * _u[i][j] + cs * _u[i][k - 1];
                                    _u[i][j] = t;
								}
							}
						}
					}
					break;
						
					// Perform one qr step.
					case 3:  
					{
						// Calculate the shift.
                        double scale = Math.Max(Math.Max(Math.Max(Math.Max(Math.Abs(_singularVector[p - 1]), Math.Abs(_singularVector[p - 2])), Math.Abs(e[p - 2])), Math.Abs(_singularVector[k])), Math.Abs(e[k]));
                        double sp = _singularVector[p - 1] / scale;
                        double spm1 = _singularVector[p - 2] / scale;
						double epm1 = e[p - 2] / scale;
                        double sk = _singularVector[k] / scale;
						double ek = e[k] / scale;
						double b = ((spm1 + sp) * (spm1 - sp) + epm1 * epm1) / 2.0;
						double c = (sp * epm1) * (sp * epm1);
						double shift = 0.0;
						if ((b != 0.0) | (c != 0.0))
						{
							shift = (double)Math.Sqrt(b * b + c);
							if (b < 0.0)
							{
								shift = - shift;
							}
							shift = c / (b + shift);
						}
						double f = (sk + sp) * (sk - sp) + shift;
						double g = sk * ek;
						
						// Chase zeros.
						
						for (int j = k; j < p - 1; j++)
						{
							double t = Functions.GeometryFunctions.Hypot(f, g);
							double cs = f / t;
							double sn = g / t;
							if (j != k)
							{
								e[j - 1] = t;
							}
                            f = cs * _singularVector[j] + sn * e[j];
                            e[j] = cs * e[j] - sn * _singularVector[j];
                            g = sn * _singularVector[j + 1];
                            _singularVector[j + 1] = cs * _singularVector[j + 1];
							if (wantv)
							{
                                for (int i = 0; i < _n; i++)
								{
                                    t = cs * _v[i][j] + sn * _v[i][j + 1];
                                    _v[i][j + 1] = (-sn) * _v[i][j] + cs * _v[i][j + 1];
									_v[i][j] = t;
								}
							}
							t = Functions.GeometryFunctions.Hypot(f, g);
							cs = f / t;
							sn = g / t;
                            _singularVector[j] = t;
                            f = cs * e[j] + sn * _singularVector[j + 1];
                            _singularVector[j + 1] = (-sn) * e[j] + cs * _singularVector[j + 1];
							g = sn * e[j + 1];
							e[j + 1] = cs * e[j + 1];
                            if (wantu && (j < _m - 1))
							{
                                for (int i = 0; i < _m; i++)
								{
                                    t = cs * _u[i][j] + sn * _u[i][j + 1];
                                    _u[i][j + 1] = (-sn) * _u[i][j] + cs * _u[i][j + 1];
                                    _u[i][j] = t;
								}
							}
						}
						e[p - 2] = f;
						iter = iter + 1;
					}
					break;
					
					// Convergence.
					case 4:  
					{
						// Make the singular values positive.
                        if (_singularVector[k] <= 0.0)
						{
                            _singularVector[k] = (_singularVector[k] < 0.0 ? -_singularVector[k] : 0.0);
							if (wantv)
							{
								for (int i = 0; i <= pp; i++)
								{
                                    _v[i][k] = -_v[i][k];
								}
							}
						}
						
						// Order the singular values.
						
						while (k < pp)
						{
                            if (_singularVector[k] >= _singularVector[k + 1])
							{
								break;
							}
                            double t = _singularVector[k];
                            _singularVector[k] = _singularVector[k + 1];
                            _singularVector[k + 1] = t;
                            if (wantv && (k < _n - 1))
							{
                                for (int i = 0; i < _n; i++)
								{
                                    t = _v[i][k + 1]; _v[i][k + 1] = _v[i][k]; _v[i][k] = t;
								}
							}
                            if (wantu && (k < _m - 1))
							{
                                for (int i = 0; i < _m; i++)
								{
                                    t = _u[i][k + 1]; _u[i][k + 1] = _u[i][k]; _u[i][k] = t;
								}
							}
							k++;
						}
						iter = 0;
						p--;
					}
					break;
				}
			}
		}

		#endregion

        ///// <summary>
        /////   Solves a linear equation system of the form AX = B.
        ///// </summary>
        ///// <param name="value">Parameter B from the equation AX = B.</param>
        ///// <returns>The solution X from equation AX = B.</returns>
        //public double[][] Solve(double[][] value)
        //{
        //    // Additionally an important property is that if there does not exists a solution
        //    // when the matrix A is singular but replacing 1/Li with 0 will provide a solution
        //    // that minimizes the residue |AX -Y|. SVD finds the least squares best compromise
        //    // solution of the linear equation system. Interestingly SVD can be also used in an
        //    // over-determined system where the number of equations exceeds that of the parameters.

        //    // L is a diagonal matrix with non-negative matrix elements having the same
        //    // dimension as A, Wi ? 0. The diagonal elements of L are the singular values of matrix A.

        //    double[][] Y = value;

        //    // Create L*, which is a diagonal matrix with elements
        //    //    L*[i] = 1/L[i]  if L[i] < e, else 0, 
        //    // where e is the so-called singularity threshold.

        //    // In other words, if L[i] is zero or close to zero (smaller than e),
        //    // one must replace 1/L[i] with 0. The value of e depends on the precision
        //    // of the hardware. This method can be used to solve linear equations
        //    // systems even if the matrices are singular or close to singular.

        //    //singularity threshold
        //    double e = Threshold;


        //    int scols = s.Length;
        //    double[,] Ls = new double[scols, scols];
        //    for (int i = 0; i < s.Length; i++)
        //    {
        //        if (System.Math.Abs(s[i]) <= e)
        //            Ls[i, i] = 0.0;
        //        else Ls[i, i] = 1.0 / s[i];
        //    }

        //    //(V x L*) x Ut x Y
        //    double[,] VL = v.Multiply(Ls);

        //    //(V x L* x Ut) x Y
        //    int vrows = v.GetLength(0);
        //    int urows = u.GetLength(0);
        //    double[,] VLU = new double[vrows, urows];
        //    for (int i = 0; i < vrows; i++)
        //    {
        //        for (int j = 0; j < urows; j++)
        //        {
        //            double sum = 0;
        //            for (int k = 0; k < urows; k++)
        //                sum += VL[i, k] * u[j, k];
        //            VLU[i, j] = sum;
        //        }
        //    }

        //    //(V x L* x Ut x Y)
        //    return VLU.Multiply(Y);
        //}

        ///// <summary>
        /////   Solves a linear equation system of the form Ax = b.
        ///// </summary>
        ///// <param name="value">The b from the equation Ax = b.</param>
        ///// <returns>The x from equation Ax = b.</returns>
        //public double[] Solve(double[] value)
        //{
        //    // Additionally an important property is that if there does not exists a solution
        //    // when the matrix A is singular but replacing 1/Li with 0 will provide a solution
        //    // that minimizes the residue |AX -Y|. SVD finds the least squares best compromise
        //    // solution of the linear equation system. Interestingly SVD can be also used in an
        //    // over-determined system where the number of equations exceeds that of the parameters.

        //    // L is a diagonal matrix with non-negative matrix elements having the same
        //    // dimension as A, Wi ? 0. The diagonal elements of L are the singular values of matrix A.

        //    //singularity threshold
        //    double e = Threshold;

        //    double[] Y = value;

        //    // Create L*, which is a diagonal matrix with elements
        //    //    L*i = 1/Li  if Li = e, else 0, 
        //    // where e is the so-called singularity threshold.

        //    // In other words, if Li is zero or close to zero (smaller than e),
        //    // one must replace 1/Li with 0. The value of e depends on the precision
        //    // of the hardware. This method can be used to solve linear equations
        //    // systems even if the matrices are singular or close to singular.


        //    int scols = s.Length;

        //    double[,] Ls = new double[scols, scols];
        //    for (int i = 0; i < s.Length; i++)
        //    {
        //        if (System.Math.Abs(s[i]) <= e)
        //            Ls[i, i] = 0;
        //        else Ls[i, i] = 1.0 / s[i];
        //    }

        //    //(V x L*) x Ut x Y
        //    double[,] VL = v.Multiply(Ls);

        //    //(V x L* x Ut) x Y
        //    int urows = u.GetLength(0);
        //    int vrows = v.GetLength(0);
        //    double[,] VLU = new double[vrows, urows];
        //    for (int i = 0; i < vrows; i++)
        //    {
        //        for (int j = 0; j < urows; j++)
        //        {
        //            double sum = 0;
        //            for (int k = 0; k < scols; k++)
        //                sum += VL[i, k] * u[j, k];
        //            VLU[i, j] = sum;
        //        }
        //    }

        //    //(V x L* x Ut x Y)
        //    return VLU.Multiply(Y);
        //}

        ///// <summary>
        /////   Computes the (pseudo-)inverse of the matrix given to the Singular value decomposition.
        ///// </summary>
        //public double[,] Inverse()
        //{
        //    double e = Threshold;

        //    // X = V*S^-1
        //    int vrows = v.GetLength(0);
        //    int vcols = v.GetLength(1);
        //    double[,] X = new double[vrows, s.Length];
        //    for (int i = 0; i < vrows; i++)
        //    {
        //        for (int j = 0; j < vcols; j++)
        //        {
        //            if (System.Math.Abs(s[j]) > e)
        //                X[i, j] = v[i, j] / s[j];
        //        }
        //    }

        //    // Y = X*U'
        //    int urows = u.GetLength(0);
        //    int ucols = u.GetLength(1);
        //    double[,] Y = new double[vrows, urows];
        //    for (int i = 0; i < vrows; i++)
        //    {
        //        for (int j = 0; j < urows; j++)
        //        {
        //            double sum = 0;
        //            for (int k = 0; k < ucols; k++)
        //                sum += X[i, k] * u[j, k];
        //            Y[i, j] = sum;
        //        }
        //    }

        //    return Y;
        //}
	}
}